/***************************************************************************************************************************
 * px4_pos_estimator.cpp
 *
 * Author: SFL
 *
 * Update Time: 2021.4.10
 *
 * 说明: 无人机位置估计程序，在GPS下的位置信息整合
 *
***************************************************************************************************************************/


//头文件
#include <ros/ros.h>

#include <wrzf_pkg/DroneState.h>
#include <geometry_msgs/PoseStamped.h>
#include <state_from_mavros.h>
// #include <LowPassFilter.h>

using namespace std;
// int Use_mocap_raw;
// int linear_window;
// int angular_window;
// float T;//一阶低通滤波器时间常数
ros::Publisher drone_state_pub;
wrzf_pkg::DroneState _DroneState;  
//---------------------------------------无人机位置及速度--------------------------------------------
Eigen::Vector3d pos_drone_fcu;                           //无人机当前位置 (来自fcu)
Eigen::Vector3d vel_drone_fcu;                           //无人机当前速度 (来自fcu)
Eigen::Vector3d Att_fcu;                               //无人机当前欧拉角(来自fcu)
Eigen::Vector3d Att_rate_fcu;

void printf_info();

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4_pos_estimator");
    ros::NodeHandle nh("~");

    // nh.param("pos_estimator/Use_mocap_raw", Use_mocap_raw, 1);
    // nh.param("pos_estimator/linear_window", linear_window, 3);
    // nh.param("pos_estimator/angular_window", angular_window,3);
    // nh.param<float>("pos_estimator/noise_T", T, 0.2);

    drone_state_pub = nh.advertise<wrzf_pkg::DroneState>("/px4_command/drone_state", 1);

    // 用于与mavros通讯的类，通过mavros接收来至飞控的消息【飞控->mavros->本程序】
    state_from_mavros _state_from_mavros;

    ros::Rate rate(100.0);//100Hz 所以采样周期是0.01s
    // float dt = 0.01;//采样周期是0.01s

    while (ros::ok())
    {
        ros::spinOnce();

        _DroneState = _state_from_mavros._DroneState;
        _DroneState.header.stamp = ros::Time::now();

        for (int i=0;i<3;i++)
        {
            pos_drone_fcu[i] = _DroneState.position[i];                           
            vel_drone_fcu[i] = _DroneState.velocity[i];                           
            Att_fcu[i] = _DroneState.attitude[i];  
            Att_rate_fcu[i] = _DroneState.attitude_rate[i];  
        }
        // //飞行器位置速度状态数据（从飞控中读出）
    
        // for(int i = 0; i < 3; i++)
        // {
        //     _DroneState.position[i] = pos_drone_fcu[i];
        //     _DroneState.velocity[i] = vel_drone_fcu[i];
        // }
        
        // // 姿态数据（从飞控中读出）
        // for(int i = 0; i < 3; i++)
        // {
        //     _DroneState.attitude[i] = Att_fcu[i];
        //     _DroneState.attitude_rate[i] = Att_rate_fcu[i];
        // }
        // _DroneState.attitude_q.w = _state_from_mavros._DroneState.attitude_q.w;
        // _DroneState.attitude_q.x = _state_from_mavros._DroneState.attitude_q.x;
        // _DroneState.attitude_q.y = _state_from_mavros._DroneState.attitude_q.y;
        // _DroneState.attitude_q.z = _state_from_mavros._DroneState.attitude_q.z;

        drone_state_pub.publish(_DroneState);

        printf_info();
        rate.sleep();
    }
    


    return 0;

}


void printf_info()
{
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>PX4_POS_ESTIMATOR<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

        cout <<">>>>>>>>>>>>>>>>>>>>>>>>FCU Info [Drone State]<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
        cout << "Connect State : " ;
        //是否和飞控建立起连接
        if (_DroneState.connected == true)
        {
            cout << " [ Connected ]  " << endl;
        }
        else
        {
            cout << " [ Unconnected ]  " << endl;
        }
        cout << "Armed : " ;
        if (_DroneState.armed == true)
        {
            cout << "  [ Armed ]   " << endl;
        }
        else
        {
            cout << "  [ DisArmed ]   " << endl;
        }
        cout << "Control Mode :  [ " << _DroneState.mode << " ]" << endl;
        cout << "GPS Status : " ;
        if (_DroneState.gps_status == -1)
        {
            cout << "  [ GPS STATUS NO FIX ]   " << endl;
        }
        else
        {
            cout << "  [ GPS STATUS FIX ]   " << endl;
        }
        cout <<">>>>>>>>>>>>>>>>>>>>>>>>FCU Info [Global Position]<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
        cout << "Latitude : " << _DroneState.latitude << endl;
        cout << "Longitude : " << _DroneState.longitude << endl;
        cout << "Altitude : " << _DroneState.altitude <<endl;
        cout <<">>>>>>>>>>>>>>>>>>>>>>>>FCU Info [Fake ENU Frame]<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
        cout << "Pos_fcu [X Y Z] : " << pos_drone_fcu[0] << " [ m ] "<< pos_drone_fcu[1] <<" [ m ] "<< pos_drone_fcu[2] <<" [ m ] "<<endl;
        cout << "Vel_fcu [X Y Z] : " << vel_drone_fcu[0] << " [m/s] "<< vel_drone_fcu[1] <<" [m/s] "<< vel_drone_fcu[2] <<" [m/s] "<<endl;
        cout << "Vel_body [X Y Z] : " << _DroneState.velocity_body[0] << " [m/s] "<< _DroneState.velocity_body[1] <<" [m/s] "<< _DroneState.velocity_body[2] <<" [m/s] "<<endl;
        cout << "Att_fcu [R P Y] : " << Att_fcu[0] * 180/M_PI <<" [deg] "<< Att_fcu[1] * 180/M_PI << " [deg] "<< Att_fcu[2] * 180/M_PI<<" [deg] "<<endl;
        cout << "Att_rate_fcu [R P Y] : " << Att_rate_fcu[0] * 180/M_PI <<" [deg/s] "<< Att_rate_fcu[1] * 180/M_PI << " [deg/s] "<< Att_rate_fcu[2] * 180/M_PI<<" [deg/s] "<<endl;

}